from libs.scservo_sdk.scservo_def import *
from libs.scservo_sdk.sms_sts import sms_sts
from libs.scservo_sdk.port_handler import PortHandler
class motor_SM150():
	def __init__(self, DEVICENAME):
		# Default setting
		self.SCS_ID = 1  # SCServo ID : 1
		self.BAUDRATE = 115200  # SCServo default baudrate : 1000000
		self.DEVICENAME = DEVICENAME #'COM1'  # Check which port is being used on your controller
		# ex) Windows: "COM1"   Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*"
		self.SCS_MINIMUM_POSITION_VALUE = 0  # SCServo will rotate between this value
		self.SCS_MAXIMUM_POSITION_VALUE = 4095
		self.SCS_MOVING_SPEED = 2400  # 速度
		self.SCS_MOVING_ACC = 50  # 加速度

		self.portHandler = PortHandler(self.DEVICENAME)
		self.packetHandler = sms_sts(self.portHandler)

	def motor_connect(self):
		#连接端口
		if self.portHandler.openPort():
			print("Succeeded to open the port")
			#发出信号，把连接按钮变为断开
		else:
			print("Failed to open the port")
			#右上角弹出连接失败
		#设置波特率
		if self.portHandler.setBaudRate(self.BAUDRATE):
			print("Succeeded to change the baudrate")
			#发出信号，把红色未连接变为以115200波特率连接，并右上角弹出连接成功
			return True
		else:
			print("Failed to change the baudrate")
			# 右上角弹出连接失败
			return False
	def motor_disconncet(self):
		self.portHandler.closePort()

	def mapping_angle(self,angle :float):
		mapped_value = int((angle / 360) * 4095)
		return mapped_value

	def move_motor_to_angle(self,anlge :float):
		position = self.mapping_angle(anlge)
		scs_comm_result, scs_error = self.packetHandler.WritePosEx(self.SCS_ID,position,self.SCS_MOVING_SPEED,self.SCS_MOVING_ACC)
		if scs_comm_result != COMM_SUCCESS:
			print("%s" % self.packetHandler.getTxRxResult(scs_comm_result))
		elif scs_error != 0:
			print("%s" % self.packetHandler.getRxPacketError(scs_error))

		while 1:
			# Read SCServo present position
			scs_present_position, scs_present_speed, scs_comm_result, scs_error = self.packetHandler.ReadPosSpeed(self.SCS_ID)
			if scs_comm_result != COMM_SUCCESS:
				print(self.packetHandler.getTxRxResult(scs_comm_result))
			else:
				print("[ID:%03d] 目标角度:%d 当前角度:%d 当前速度:%d" % (self.SCS_ID, position, scs_present_position, scs_present_speed))
			if scs_error != 0:
				print(self.packetHandler.getRxPacketError(scs_error))

			# Read SCServo moving status
			moving, scs_comm_result, scs_error = self.packetHandler.ReadMoving(self.SCS_ID)
			if scs_comm_result != COMM_SUCCESS:
				print(self.packetHandler.getTxRxResult(scs_comm_result))

			if moving == 0:
				break
